/**
 * DifferentialSubsystem
 * 
 * This file contains all information relative to the Differentials.
 */

package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 * @author Jarrod Risley
 */

public class DifferentialSubsystem extends Subsystem {

    /*
     * Global Declarations
     */
    private Victor leftDifferential;
    private Victor rightDifferential;
    private DifferentialPosition positionChecker;
    
    private boolean isActive = false;
    
    public static class DifferentialPosition {
        
        private boolean state;
        private double value;
        
        // Values for low gear.
        private static final boolean LOW = false;
        private static final double LOW_VICTOR_SETTING = 0;
        
        // Values for high gear.
        private static final boolean HIGH = true;
        private static final double HIGH_VICTOR_SETTING = 1.0;
        
        // Low gear position.
        public static final DifferentialPosition LOW_GEAR =
                new DifferentialPosition(LOW, LOW_VICTOR_SETTING);
        
        // High gear position
        public static final DifferentialPosition HIGH_GEAR = 
                new DifferentialPosition(HIGH, HIGH_VICTOR_SETTING);
        
        /**
         * Constructs a DifferentialPosition (a wrapped boolean and double).
         * 
         * @param position The position - True is high gear, false is low gear.
         */
        
        private DifferentialPosition(boolean position, double voltage) {
            
            this.state = position;
            this.value = voltage;
            
        } // end DifferentialPosition Constructor
        
    } // end DifferentialPosition
    
    public void initDefaultCommand() {} // end initDefaultCommand
    
    public DifferentialPosition getDifferentialState() {
        return positionChecker;
    } // end getDifferentialState
    
    /**
     * Sets the state of the Differential with a DifferentialPosition.
     * 
     * @param position DifferentialPosition, either HIGH_GEAR or LOW_GEAR.
     */
    
    public final void setDifferentialState(DifferentialPosition position) {
        
        if (isActive) {
            
            positionChecker = position;
            
            if (position == DifferentialPosition.LOW_GEAR
                    && getDifferentialState() != position) {
                
                moveVictors(DifferentialPosition.LOW_GEAR.value);
                
            } else if (position == DifferentialPosition.HIGH_GEAR
                    && getDifferentialState() != position) {
                
                moveVictors(DifferentialPosition.HIGH_GEAR.value);
                
            } // end if - else
            
        } // end outer if
        
    } // end setDifferentialState
    
    /**
     * Moves the motors for the differential
     * @param value Grab this value from a DifferentialPosition.
     */
    
    private void moveVictors(double value) {
        
        leftDifferential.set(value);
        rightDifferential.set(value);
        
    } // end moveVictors
    
    /*
     * Constructor
     */
    
    /**
     * Creates the DifferentialSubsystem.
     * 
     * @param name The name of the subsystem.
     */
    
    public DifferentialSubsystem(final String name, int leftVictor, 
            int rightVictor) {
        
        System.out.println("Creating DifferentialSubsystem " + name + "...");
        
        leftDifferential = new Victor(leftVictor);
        rightDifferential = new Victor(rightVictor);
        
        isActive = true;
        
        System.out.println("Setting " + name + "into Low Gear.");
        setDifferentialState(DifferentialPosition.LOW_GEAR);
        
    } // end DIfferentialSubsystem Constructor
    
} // end DifferentialSubsystem

